#include "astar.h"


using namespace std;


//Constructor that sets the start and goal points and the map
aStar::aStar(int* pmap, int xStart, int yStart, int xGoal, int yGoal)
{
	add.first = 0;
	add.second = 0.0;
	modify.first = 0;
	modify.second = 0.0;
	selectLowest.first = 0;
	selectLowest.second = 0.0;
	find.first = 0;
	find.second = 0.0;

	this->pmap = pmap;
	
	start = allocateNode(xStart, yStart);
	goal = allocateNode(xGoal, yGoal);
}


//Calculates the estimated (heuristic) distance from the coordinates (x,y) to the coordinates of the node
float aStar::estimatedDistanceTo(int x, int y, Node* node)
{
	//Calculate the estimated distance
	float h = (fabs((float)x - (float)node->getX()) + fabs((float)y - (float)node->getY())) * 1.3;

	//Break straigth lines using tie breaking techniques
	//It gives a lower cost to the nodes that are in diagonal
	//h += 0.001*(rand()%4);
	float dx = fabs(x - goal->getX());
	float dy = fabs(y - goal->getY());
	float diagonal = fabs(dx-dy);
	h += diagonal * 0.001;
	return h;
}


//Function that executes the A* algorithm
template <class data, class pointer>
results* aStar::execute()
{	
	timer t;
	t.startTimer();

	//Set to 0 the cost of the start node and to 1 the cost of the goal node
	*(pmap + start->getX() + start->getY()*width) = 0;
	*(pmap + goal->getX() + goal->getY()*width) = 1;

	visitedNodes = 1;

	//Create the data structures for the opened and closed nodes
	data openedNodes;
	data closedNodes;
	openedNodes.allocate(width, height);
	closedNodes.allocate(width, height);

	//Add the start node to the openedNodes
	timer t2;
	t2.startTimer();
	openedNodes.addNode(start);
	t2.stopTimer();
	double diffticks = t2.getElapsedTime() * 1000;
	add.second = (double)add.first / (double)(add.first + 1) * add.second + (double)1 / (double)(add.first + 1) * diffticks;
	add.first++;

	//Initialise the values f, g and h of the start node
	start->setG(0);
	start->setH(estimatedDistanceTo(start->getX(), start->getY(), goal));
	start->setF(start->getG() + start->getH());

	while(!openedNodes.empty())
   	{
		//Obtain the opened node with the lowest score and erase it from this structure
		t2.startTimer();
		Node* currentNode = openedNodes.selectNodeLowestScore();
		t2.stopTimer();
		diffticks = t2.getElapsedTime()*1000;
		selectLowest.second = (double)selectLowest.first / (double)(selectLowest.first + 1) * selectLowest.second + (double)1 / (double)(selectLowest.first + 1) * diffticks;
		selectLowest.first++;

		//If the current node is the goal node, the algorithm is finished
		if(currentNode->sameCoordinates(goal))
		{
			//Set to 5 the start node and to 6 the goal node
			*(pmap + start->getX() + start->getY()*width) = 5;
			*(pmap + goal->getX() + goal->getY()*width) = 6;

			t.stopTimer();
			diffticks = t.getElapsedTime();
			results* res = createResults(currentNode, typeid(openedNodes), "A*", diffticks);
			openedNodes.freeNodes();
			closedNodes.freeNodes();

			return res;
		}

		//Add the current node to the closed nodes set
		t2.startTimer();
		closedNodes.addNode(currentNode);
		t2.stopTimer();
		diffticks = t2.getElapsedTime() * 1000;
		add.second = (double)add.first / (double)(add.first + 1) * add.second + (double)1 / (double)(add.first + 1) * diffticks;
		add.first++;

		//Obtain a vector containing the coordinates of the successors of the current node
		vector<coord>* successors = getSuccessors(currentNode);

		for(vector<coord>::iterator it = successors->begin(); it != successors->end(); ++it)
		{
			//Calculate the cost of the successor
			float g = currentNode->getG() + *(pmap + it->first + it->second*width);
			float h = estimatedDistanceTo(it->first, it->second, goal);
			float f = g + h;

			bool nodeNotCreated = false;

			//Check if it is a closed node
			t2.startTimer();
			pair<pointer, Node*> p;
			p = closedNodes.findNode(it->first, it->second);
			t2.stopTimer();
			diffticks = t2.getElapsedTime() * 1000;
			find.second = (double)find.first / (double)(find.first + 1) * find.second + (double)1 / (double)(find.first + 1) * diffticks;
			find.first++;
			
			//If it is not a closed node, then we need to check if it is on the opened nodes list
			Node* n;
			if(!p.second)
			{
				//Check if it is a openeded node
				t2.startTimer();
				p = openedNodes.findNode(it->first, it->second);		
				t2.stopTimer();
				diffticks = t2.getElapsedTime() * 1000;
				find.second = (double)find.first / (double)(find.first + 1) * find.second + (double)1 / (double)(find.first + 1) * diffticks;
				find.first++;
			
				//If it is not a opened node, then we need to add it to the opened nodes list	
				if(!p.second)
				{
					nodeNotCreated = true;
				}
				//If it is a opened node, we should check its cost
				else
				{
					//If this node is currently reached with a lower cost than when it was closed, we modify its cost
					n = p.second;	
					if(f < n->getF())
					{
						n->setG(g);
						n->setH(h);
						n->setF(f);
						n->setParent(currentNode);
						t2.startTimer();
						openedNodes.modifyNode(p.first);
						t2.stopTimer();
						diffticks = t2.getElapsedTime() * 1000;
						modify.second = (double)modify.first / (double)(modify.first + 1) * modify.second + (double)1 / (double)(modify.first + 1) * diffticks;
						modify.first++;
					}
				}
			}
			//If it is a closed node, we should check its cost
			else
			{
				//If this node is currently reached with a lower cost than when it was closed, we remove it from closed nodes
				//We need to add it now to the opened nodes list
				n = p.second;
				if(f < n->getF())
				{
					closedNodes.remove(p.first);

					n->setG(g);
					n->setH(h);
					n->setF(f);
					n->setParent(currentNode);

					t2.startTimer();
					openedNodes.addNode(n);
					t2.stopTimer();
					diffticks = t2.getElapsedTime() * 1000;
					add.second = (double)add.first / (double)(add.first + 1) * add.second + (double)1 / (double)(add.first + 1) * diffticks;
					add.first++;
				}
			}

			//If the node was not either in the closed or opened nodes, we should create it and add it to the opened nodes
			if(nodeNotCreated)
			{
				visitedNodes++;
				Node* n2 = allocateNode(it->first, it->second);
				n2->setG(g);
				n2->setH(h);
				n2->setF(f);
				n2->setParent(currentNode);
				t2.startTimer();
				openedNodes.addNode(n2);
				t2.stopTimer();
				diffticks = t2.getElapsedTime() * 1000;
				add.second = (double)add.first / (double)(add.first + 1) * add.second + (double)1 / (double)(add.first + 1) * diffticks;
				add.first++;
			}
		}	
   	}
	//Set to 5 the start node and to 6 the goal node
	*(pmap + start->getX() + start->getY()*width) = 5;
	*(pmap + goal->getX() + goal->getY()*width) = 6;

	t.stopTimer();
	diffticks = t.getElapsedTime();
	results* res = createResults(0, typeid(openedNodes), "A*", diffticks);

	//If we arrive to this point, the algorithm has not found a path
	openedNodes.freeNodes();
	closedNodes.freeNodes();

	return res;
}

template results* aStar::execute<unsortedVector, std::vector<Node*>::iterator>();
template results* aStar::execute<sortedVectorByF, std::vector<Node*>::iterator>();
template results* aStar::execute<sortedVectorByXY, std::vector<Node*>::iterator>();
template results* aStar::execute<skipList, skipListItem*>();
template results* aStar::execute<hashTable, Node**>();
template results* aStar::execute<hashTableSortedVector, Node**>();